package pid

import (
	"errors"
	"fmt"
	"math"
	"sync"
	"time"
)

type PID struct {
	Kp           float64
	Ki           float64
	Kd           float64
	outputLimits [2]float64
	integralLim  [2]float64
	deadZone     float64
	prevError    float64
	prevMeasured float64
	integral     float64
	lastTime     time.Time
	mu           sync.Mutex
}

func NewPID(kp, ki, kd float64) *PID {
	return &PID{
		Kp:           kp,
		Ki:           ki,
		Kd:           kd,
		outputLimits: [2]float64{math.Inf(-1), math.Inf(1)},
		integralLim:  [2]float64{math.Inf(-1), math.Inf(1)},
		lastTime:     time.Now(),
	}
}

func (p *PID) Compute(setpoint, measured float64) float64 {
	p.mu.Lock()
	defer p.mu.Unlock()

	now := time.Now()
	dt := now.Sub(p.lastTime).Seconds()
	p.lastTime = now

	// 死区处理
	if p.deadZone > 0 && abs(setpoint-measured) < p.deadZone {
		fmt.Println("dead zone:", p.deadZone, setpoint, measured)
		return 0
	}

	error := setpoint - measured
	p.integral += error * dt
	p.integral = clamp(p.integral, p.integralLim[0], p.integralLim[1])

	derivative := (measured - p.prevMeasured) / dt
	p.prevMeasured = measured
	p.prevError = error

	output := p.Kp*error + p.Ki*p.integral - p.Kd*derivative
	return clamp(output, p.outputLimits[0], p.outputLimits[1])
}

func abs(x float64) float64 {
	if x < 0 {
		return -x
	}
	return x
}

func clamp(value, min, max float64) float64 {
	if value < min {
		return min
	}
	if value > max {
		return max
	}
	return value
}

// SetOutputLimits 设置输出限幅
func (pid *PID) SetOutputLimits(min, max float64) error {
	if min >= max {
		return errors.New("invalid limits")
	}
	pid.outputLimits = [2]float64{min, max}
	return nil
}

// SetIntegralLimits 设置积分限幅
func (pid *PID) SetIntegralLimits(min, max float64) error {
	if min >= max {
		return errors.New("invalid limits")
	}
	pid.integralLim = [2]float64{min, max}
	return nil
}

// SetDeadZone 设置死区范围
func (pid *PID) SetDeadZone(value float64) {
	pid.deadZone = math.Abs(value)
}

// Reset 重置控制器状态
func (pid *PID) Reset() {
	pid.mu.Lock()
	defer pid.mu.Unlock()

	pid.prevError = 0
	pid.integral = 0
	pid.lastTime = time.Now()
}
